package com.dji.activationDemo.messaging;

import com.dji.activationDemo.Constant;
import com.dji.activationDemo.uitls.RandomNum;
import com.dji.activationDemo.uitls.WriteLog;
import com.dji.activationDemo.websocket.WsManager;

import java.util.ArrayList;
import java.util.List;

import dji.common.error.DJIError;
import dji.common.mission.waypoint.Waypoint;
import dji.common.mission.waypoint.WaypointAction;
import dji.common.mission.waypoint.WaypointActionType;
import dji.common.mission.waypoint.WaypointMission;
import dji.common.mission.waypoint.WaypointMissionDownloadEvent;
import dji.common.mission.waypoint.WaypointMissionExecutionEvent;
import dji.common.mission.waypoint.WaypointMissionFinishedAction;
import dji.common.mission.waypoint.WaypointMissionFlightPathMode;
import dji.common.mission.waypoint.WaypointMissionGotoWaypointMode;
import dji.common.mission.waypoint.WaypointMissionHeadingMode;
import dji.common.mission.waypoint.WaypointMissionState;
import dji.common.mission.waypoint.WaypointMissionUploadEvent;
import dji.common.mission.waypoint.WaypointTurnMode;
import dji.common.util.CommonCallbacks;
import dji.sdk.mission.MissionControl;
import dji.sdk.mission.waypoint.WaypointMissionOperator;
import dji.sdk.mission.waypoint.WaypointMissionOperatorListener;

public class AttitudeWaypointMission {

    private WaypointMissionOperator waypointMissionOperator = MissionControl.getInstance().getWaypointMissionOperator();

    RandomNum randomNum = new RandomNum();

    String[] waypoints;
    int num;
    int taskLength;
    int start = 1;
    int end;

    String sign;
    String missionName;
    String sendName;

    //加载任务
    private void loadMission(WaypointMission waypointMission, String sign) {
        System.out.println("进入加载");
        if (waypointMission != null) {
            DJIError djiError = waypointMissionOperator.loadMission(waypointMission);
            if (djiError == null) {
                uploadMission(sign);
                System.out.println("Mission is loaded successfully");
            } else {
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"加载任务失败：" + djiError.getDescription() + "\"}}");
                sendMessage(sendName + "*error*" + sign + "*0*加载任务失败：" + djiError.getDescription());
                System.out.println("加载任务报错了:" + djiError.toString());
            }
        } else {
            sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"未收到点\"}}");
        }
    }

    //上传任务
    private void uploadMission(String sign) {
        if (WaypointMissionState.READY_TO_RETRY_UPLOAD.equals(waypointMissionOperator.getCurrentState()) || WaypointMissionState.READY_TO_UPLOAD.equals(waypointMissionOperator.getCurrentState())) {
            waypointMissionOperator.uploadMission(new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {
                    if (djiError == null) {
                        System.out.println("任务上传成功");
                        startMission(sign);
                    } else {
                        sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"上传任务失败：" + djiError.getDescription() + "\"}}");
                        System.out.println("上传任务报错：" + djiError.toString());
                    }
                }
            });
        } else {
            System.out.println("Wait for mission to be loaded");
        }
    }

    //开始执行任务
    private void startMission(String sign) {
        if (WaypointMissionState.READY_TO_EXECUTE.equals(waypointMissionOperator.getCurrentState())) {
            System.out.println("任务上传完成，开始执行");
        } else {
            System.out.println("等待上传完成");
            try {
                Thread.sleep(4000);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
        waypointMissionOperator.startMission(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (djiError != null) {
                    sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"执行报错：" + djiError.getDescription() + "\"}}");
                    System.out.println("执行报错:" + djiError.toString());
                } else {
                    sendMessage(sendName + "*" + sign + "*0*{code:\"received\"}");
                    System.out.println("开始执行任务");
                }
            }
        });
    }

    public void setUpListener() {



        Constant.waypointMissionOperatorListener = new WaypointMissionOperatorListener() {
            @Override
            public void onDownloadUpdate(WaypointMissionDownloadEvent waypointMissionDownloadEvent) {
//                System.out.println("航线任务下载成功");
//                WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务下载成功");
            }

            @Override
            public void onUploadUpdate(WaypointMissionUploadEvent waypointMissionUploadEvent) {
//                System.out.println("航线任务上传成功");
//                WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务上传成功");
            }

            @Override
            public void onExecutionUpdate(WaypointMissionExecutionEvent waypointMissionExecutionEvent) {
                int n = waypointMissionExecutionEvent.getProgress().targetWaypointIndex;
                boolean isWR = waypointMissionExecutionEvent.getProgress().isWaypointReached;
                WriteLog.writeLog("第"+n+"个航点，飞行状态："+waypointMissionExecutionEvent.getProgress().executeState.toString()+",isWaypointReached:"+isWR);



            }

            @Override
            public void onExecutionStart() {
                sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*航线任务开始");
//                WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务开始");
                System.out.println("监听到航点任务开始");
            }

            @Override
            public void onExecutionFinish(DJIError djiError) {
                if (djiError == null) {
                    if (Constant.waypointExecution) {
                        System.out.println("任务还剩次数：" + num);
                        if (num == 0) {
                            System.out.println("航点结束");
                            sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*航线任务结束");
                        } else {
                            sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*准备开始下一个小任务，任务还剩次数：" + num);
                            nextTask();
                        }
                    } else {
                        System.out.println("航线任务结束");
                        sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*航线任务结束");
                    }
                } else {
                    System.out.println("结束失败:" + djiError.getDescription());
                }

            }
        };
        waypointMissionOperator.addListener(Constant.waypointMissionOperatorListener);
    }


    public void nextTask() {
        System.out.println("准备下一次任务");
        if (num == 1) {
            start = end + 1;
            end = waypoints.length - 1;
            num--;
        } else {
            start = end + 1;
            end = end + taskLength;
            num--;
        }
        WaypointMission waypointMission = null;
        if (missionName.equals("multitaskingFlight")) {
            waypointMission = createPitchWaypointMission2();
        }else if (missionName.equals("waypointVideoDemo")){
            waypointMission = createPitchWaypointMission3();
        }
        loadMission(waypointMission, sign);
    }


    boolean clm;

    //多次多角度拍照任务
    public void multitaskingFlight(String paramString, String sign, String name) {
        Constant.waypointExecution = true;
        sendName = name;
        missionName = "multitaskingFlight";
        start = 1;
        if (Constant.waypointMissionOperatorListener != null) {
            waypointMissionOperator.removeListener(Constant.waypointMissionOperatorListener);
        } else {
            setUpListener();
        }

        waypoints = paramString.split("\\|");
        WaypointMission waypointMission = null;
        this.sign = sign;
        System.out.println("航点数量：" + (waypoints.length - 1));
        num = (int) Math.ceil((waypoints.length - 1) / (double) 99);
        System.out.println("任务次数：" + num);
        if (num <= 1) {
            end = waypoints.length - 1;
            waypointMission = createPitchWaypointMission2();
            num--;
        } else {
            taskLength = (waypoints.length - 1) / num;
            end = taskLength;
            if (checkLoadMission()){
                if (clm) {
                    num = (int) Math.ceil((waypoints.length - 1) / (double) 99);
                    start = 1;
                    taskLength = (waypoints.length - 1) / num;
                    end = taskLength;
                    waypointMission = createPitchWaypointMission2();
                    num--;
                } else {
                    return;
                }
            }else {
                System.out.println("任务点数据格式不正确");
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"任务点数据格式不正确\"}}");
                return;
            }


        }
        //开始执行任务
        loadMission(waypointMission, sign);
    }


    //录像
    public void waypointVideoDemo(String paramString, String sign, String name) {
        Constant.waypointExecution = true;
        sendName = name;
        missionName = "waypointVideoDemo";
        start = 1;
        if (Constant.waypointMissionOperatorListener != null) {
            waypointMissionOperator.removeListener(Constant.waypointMissionOperatorListener);
        } else {
            setUpListener();
        }

        waypoints = paramString.split("\\|");
        WaypointMission waypointMission = null;
        this.sign = sign;
        System.out.println("航点数量：" + (waypoints.length - 1));
        num = (int) Math.ceil((waypoints.length - 1) / (double) 99);
        System.out.println("任务次数：" + num);
        if (num <= 1) {
            end = waypoints.length - 1;
            waypointMission = createPitchWaypointMission3();
            num--;
        } else {
            taskLength = (waypoints.length - 1) / num;
            end = taskLength;
            if (checkLoadMission()){
                if (clm) {
                    num = (int) Math.ceil((waypoints.length - 1) / (double) 99);
                    start = 1;
                    taskLength = (waypoints.length - 1) / num;
                    end = taskLength;
                    waypointMission = createPitchWaypointMission3();
                    num--;
                } else {
                    return;
                }
            }else {
                System.out.println("任务点数据格式不正确");
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"任务点数据格式不正确\"}}");
                return;
            }


        }
        //开始执行任务
        loadMission(waypointMission, sign);
    }

    //规划航点
    private WaypointMission createPitchWaypointMission2() {
        try {
            WaypointMission.Builder builder = new WaypointMission.Builder();
            builder.autoFlightSpeed(2f);
            builder.maxFlightSpeed(5f);
            builder.setExitMissionOnRCSignalLostEnabled(false);
            builder.finishedAction(WaypointMissionFinishedAction.NO_ACTION);//任务完成：悬停
            builder.flightPathMode(WaypointMissionFlightPathMode.NORMAL);//航线模式：直线
            builder.gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.SAFELY);//安全前往航点
            builder.headingMode(WaypointMissionHeadingMode.AUTO);//航向模式:与飞行方向一致
            List<Waypoint> waypointList = new ArrayList<>();
            for (int i = start; i <= end; i++) {
                String[] aa = waypoints[i].split(",");
                double baseLongitude = Double.parseDouble(aa[0]);
                double baseLatitude = Double.parseDouble(aa[1]);
                float baseAltitude = Float.parseFloat(aa[2]);
                Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
                waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
                if (aa.length > 3) {
//                System.out.println("点动作");
                    for (int j = 3; j < aa.length; j++) {
                        String attitudeAndGimbal = aa[j];
                        if (attitudeAndGimbal.contains("+")) {
                            String[] photograph = attitudeAndGimbal.split("\\+");
                            int attitude = Integer.parseInt(photograph[0]);
                            int gimbal = Integer.parseInt(photograph[1]);
                            waypoint.addAction(new WaypointAction(WaypointActionType.ROTATE_AIRCRAFT, attitude));
                            waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, gimbal));
                            waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 0));
                        } else {
                            int attitude = Integer.parseInt(attitudeAndGimbal);
                            waypoint.addAction(new WaypointAction(WaypointActionType.ROTATE_AIRCRAFT, attitude));
                        }
                    }
                }

                waypointList.add(waypoint);
            }
            builder.waypointList(waypointList).waypointCount(waypointList.size());
            return builder.build();
        } catch (Exception e) {
            e.printStackTrace();
            return null;
        }

    }


    //规划航点
    private WaypointMission createPitchWaypointMission3() {
        try {
            WaypointMission.Builder builder = new WaypointMission.Builder();
            builder.autoFlightSpeed(3f);
            builder.maxFlightSpeed(6f);
            builder.setExitMissionOnRCSignalLostEnabled(false);
            builder.finishedAction(WaypointMissionFinishedAction.NO_ACTION);//任务完成：悬停
            builder.flightPathMode(WaypointMissionFlightPathMode.NORMAL);//航线模式：直线
            builder.gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.SAFELY);//安全前往航点
            builder.headingMode(WaypointMissionHeadingMode.AUTO);//航向模式:与飞行方向一致
            List<Waypoint> waypointList = new ArrayList<>();
            for (int i = start; i <= end; i++) {
                String[] aa = waypoints[i].split(",");
                double baseLongitude = Double.parseDouble(aa[0]);
                double baseLatitude = Double.parseDouble(aa[1]);
                float baseAltitude = Float.parseFloat(aa[2]);
                Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
                waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
                if (aa.length > 3) {
                    for (int j = 3; j < aa.length; j++) {
                        String attitudeAndGimbal = aa[j];
                        if (attitudeAndGimbal.contains("+")) {
                            String[] photograph = attitudeAndGimbal.split("\\+");
                            int attitude = Integer.parseInt(photograph[0]);
                            int gimbal = Integer.parseInt(photograph[1]);
                            waypoint.addAction(new WaypointAction(WaypointActionType.ROTATE_AIRCRAFT, attitude));
                            waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, gimbal));
                        } else {
                            int attitude = Integer.parseInt(attitudeAndGimbal);
                            waypoint.addAction(new WaypointAction(WaypointActionType.ROTATE_AIRCRAFT, attitude));
                        }
                    }
                }

                if (i==start){
                    waypoint.addAction(new WaypointAction(WaypointActionType.START_RECORD, 0));
//                }else if (i == (end-1)){
                }else if (i == end){
                    WriteLog.writeLog("规格点从"+start+"到"+end+"个点，共"+(end-start)+"个航点");
                    waypoint.addAction(new WaypointAction(WaypointActionType.STOP_RECORD, 0));
                }

                waypointList.add(waypoint);
            }
            builder.waypointList(waypointList).waypointCount(waypointList.size());
            return builder.build();
        } catch (Exception e) {
            e.printStackTrace();
            return null;
        }

    }


    //校验加载点
    private boolean checkLoadMission() {
        System.out.println("开始：" + start + ",结束：" + end);
        WaypointMission waypointMission = null;
        if (missionName.equals("waypointVideoDemo")){
            waypointMission = createPitchWaypointMission3();
        }else{
            waypointMission = createPitchWaypointMission2();
        }

        if (waypointMission != null) {
            DJIError djiError = waypointMissionOperator.loadMission(waypointMission);
            if (djiError == null) {
                num--;
                if (num == 0) {
                    clm = true;
                } else {
                    if (num == 1) {
                        start = end + 1;
                        end = waypoints.length - 1;
                    } else {
                        start = end + 1;
                        end = end + taskLength;
                    }
                    checkLoadMission();
                }
            } else {
                clm = false;
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"校验加载任务报错了：" + djiError.getDescription() + "\"}}");
                System.out.println("校验加载任务报错了:" + djiError.toString());
                return false;
            }
        } else {
            return false;
        }
        return true;
    }


    //发送消息
    public void sendMessage(String message) {

        new Thread(new Runnable() {
            @Override
            public void run() {
                boolean start = true;
                while (start) {
                    try {
                        if (WsManager.ws.isOpen()) {
                            System.out.println("发送成功：" + message);
                            WsManager.ws.sendText(message);
                            start = false;
                        }
                        Thread.sleep(250);
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }

                }
            }
        }).start();
    }
}
